/****************************************************************************
 *
 *   Copyright (C) 2024 Zarek Chen. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file test_blocking_queue.cpp
 * @brief Unit tests for BlockingQueue
 */

#include <px4_platform_common/containers/blocking_queue.hpp>
#include "FreeRTOS.h"
#include "task.h"
#include <string.h>

extern "C" {
	void debug_printf(const char *fmt, ...);
}

// Test statistics
static int test_passed = 0;
static int test_failed = 0;

// Test assertion macro
#define TEST_ASSERT(condition, message) \
	do { \
		if (condition) { \
			test_passed++; \
		} else { \
			test_failed++; \
			debug_printf("  [FAIL] %s\n", message); \
		} \
	} while(0)

// Test 1: Basic push and pop
static void test_basic_push_pop()
{
	debug_printf("Test 1: Basic push and pop\n");

	BlockingQueue<int, 5> queue;

	queue.push(10);
	queue.push(20);
	queue.push(30);

	int val1 = queue.pop();
	int val2 = queue.pop();
	int val3 = queue.pop();

	TEST_ASSERT(val1 == 10, "First pop should return 10");
	TEST_ASSERT(val2 == 20, "Second pop should return 20");
	TEST_ASSERT(val3 == 30, "Third pop should return 30");
}

// Test 2: Fill and empty queue
static void test_fill_and_empty()
{
	debug_printf("Test 2: Fill and empty queue\n");

	BlockingQueue<int, 5> queue;

	// Fill the queue
	for (int i = 0; i < 5; i++) {
		queue.push(i * 10);
	}

	// Empty the queue
	for (int i = 0; i < 5; i++) {
		int val = queue.pop();
		TEST_ASSERT(val == i * 10, "Popped value should match pushed value");
	}
}

// Test 3: Pointer types
static void test_pointer_types()
{
	debug_printf("Test 3: Pointer types\n");

	typedef struct {
		int value;
		char name[16];
	} TestItem;

	BlockingQueue<TestItem *, 3> queue;

	TestItem item1 = { .value = 100 };
	TestItem item2 = { .value = 200 };
	TestItem item3 = { .value = 300 };
	strcpy(item1.name, "first");
	strcpy(item2.name, "second");
	strcpy(item3.name, "third");

	queue.push(&item1);
	queue.push(&item2);
	queue.push(&item3);

	TestItem *p1 = queue.pop();
	TestItem *p2 = queue.pop();
	TestItem *p3 = queue.pop();

	TEST_ASSERT(p1 == &item1, "First pop should return item1");
	TEST_ASSERT(p2 == &item2, "Second pop should return item2");
	TEST_ASSERT(p3 == &item3, "Third pop should return item3");
	TEST_ASSERT(p1->value == 100, "item1 value should be 100");
	TEST_ASSERT(strcmp(p2->name, "second") == 0, "item2 name should be 'second'");
}

// Test 4: Circular buffer wrapping
static void test_circular_wrapping()
{
	debug_printf("Test 4: Circular buffer wrapping\n");

	BlockingQueue<int, 3> queue;

	// Fill queue
	queue.push(1);
	queue.push(2);
	queue.push(3);

	// Pop 2 items
	int v1 = queue.pop();
	int v2 = queue.pop();
	TEST_ASSERT(v1 == 1 && v2 == 2, "First two pops correct");

	// Push 2 more (wraps around)
	queue.push(4);
	queue.push(5);

	// Pop all
	int v3 = queue.pop();
	int v4 = queue.pop();
	int v5 = queue.pop();

	TEST_ASSERT(v3 == 3, "Third pop should be 3");
	TEST_ASSERT(v4 == 4, "Fourth pop should be 4");
	TEST_ASSERT(v5 == 5, "Fifth pop should be 5");
}

// Test 5: Producer-Consumer pattern
static BlockingQueue<int, 10> *g_test_queue = NULL;
static volatile int g_producer_count = 0;
static volatile int g_consumer_sum = 0;

static void producer_task(void *param)
{
	int start = *(int *)param;

	for (int i = 0; i < 5; i++) {
		g_test_queue->push(start + i);
		g_producer_count++;
		vTaskDelay(pdMS_TO_TICKS(2));
	}

	vTaskDelete(NULL);
}

static void consumer_task(void *param)
{
	(void)param;

	for (int i = 0; i < 10; i++) {
		int val = g_test_queue->pop();
		g_consumer_sum += val;
		vTaskDelay(pdMS_TO_TICKS(1));
	}

	vTaskDelete(NULL);
}

static void test_producer_consumer()
{
	debug_printf("Test 5: Producer-Consumer pattern\n");

	BlockingQueue<int, 10> queue;
	g_test_queue = &queue;
	g_producer_count = 0;
	g_consumer_sum = 0;

	int start1 = 0;
	int start2 = 5;

	TaskHandle_t prod1, prod2, cons;

	xTaskCreate(producer_task, "prod1", 512, &start1, 10, &prod1);
	xTaskCreate(producer_task, "prod2", 512, &start2, 10, &prod2);
	xTaskCreate(consumer_task, "cons", 512, NULL, 10, &cons);

	// Wait for tasks to complete
	vTaskDelay(pdMS_TO_TICKS(200));

	TEST_ASSERT(g_producer_count == 10, "Producers should push 10 items");
	// Sum of 0+1+2+3+4+5+6+7+8+9 = 45
	TEST_ASSERT(g_consumer_sum == 45, "Consumer should receive sum of 45");

	g_test_queue = NULL;
}

// Test 6: Blocking behavior - consumer waits for producer
static volatile bool g_consumer_blocked = false;
static volatile bool g_consumer_unblocked = false;

static void slow_producer_task(void *param)
{
	(void)param;

	// Wait a bit to ensure consumer blocks
	vTaskDelay(pdMS_TO_TICKS(50));

	// Now push an item
	g_test_queue->push(999);

	vTaskDelete(NULL);
}

static void blocking_consumer_task(void *param)
{
	(void)param;

	g_consumer_blocked = true;
	int val = g_test_queue->pop();  // This should block
	g_consumer_unblocked = true;

	TEST_ASSERT(val == 999, "Consumer should receive 999");

	vTaskDelete(NULL);
}

static void test_blocking_behavior()
{
	debug_printf("Test 6: Blocking behavior\n");

	BlockingQueue<int, 10> queue;
	g_test_queue = &queue;
	g_consumer_blocked = false;
	g_consumer_unblocked = false;

	TaskHandle_t prod, cons;

	xTaskCreate(blocking_consumer_task, "cons", 512, NULL, 10, &cons);
	vTaskDelay(pdMS_TO_TICKS(10));  // Let consumer start and block

	TEST_ASSERT(g_consumer_blocked == true, "Consumer should be blocked");
	TEST_ASSERT(g_consumer_unblocked == false, "Consumer should still be blocked");

	xTaskCreate(slow_producer_task, "prod", 512, NULL, 10, &prod);

	// Wait for producer to unblock consumer
	vTaskDelay(pdMS_TO_TICKS(100));

	TEST_ASSERT(g_consumer_unblocked == true, "Consumer should be unblocked");

	g_test_queue = NULL;
}

// Test 7: Multiple items with different types
static void test_struct_items()
{
	debug_printf("Test 7: Struct items\n");

	typedef struct {
		int id;
		float value;
	} DataItem;

	BlockingQueue<DataItem, 5> queue;

	DataItem item1 = { .id = 1, .value = 1.5f };
	DataItem item2 = { .id = 2, .value = 2.5f };
	DataItem item3 = { .id = 3, .value = 3.5f };

	queue.push(item1);
	queue.push(item2);
	queue.push(item3);

	DataItem r1 = queue.pop();
	DataItem r2 = queue.pop();
	DataItem r3 = queue.pop();

	TEST_ASSERT(r1.id == 1 && r1.value == 1.5f, "First item correct");
	TEST_ASSERT(r2.id == 2 && r2.value == 2.5f, "Second item correct");
	TEST_ASSERT(r3.id == 3 && r3.value == 3.5f, "Third item correct");
}

// Test 8: Large queue capacity
static void test_large_capacity()
{
	debug_printf("Test 8: Large queue capacity\n");

	BlockingQueue<int, 50> queue;

	// Fill queue
	for (int i = 0; i < 50; i++) {
		queue.push(i);
	}

	// Empty queue and verify
	int errors = 0;
	for (int i = 0; i < 50; i++) {
		int val = queue.pop();
		if (val != i) {
			errors++;
		}
	}

	TEST_ASSERT(errors == 0, "All 50 items should be correct");
}

// Main test function
extern "C" void test_blocking_queue()
{
	debug_printf("\n========================================\n");
	debug_printf("  BlockingQueue Tests\n");
	debug_printf("========================================\n");

	test_passed = 0;
	test_failed = 0;

	test_basic_push_pop();
	test_fill_and_empty();
	test_pointer_types();
	test_circular_wrapping();
	test_producer_consumer();
	test_blocking_behavior();
	test_struct_items();
	test_large_capacity();

	debug_printf("========================================\n");
	debug_printf("  Test Summary\n");
	debug_printf("========================================\n");
	debug_printf("Total tests: %d\n", test_passed + test_failed);
	debug_printf("Passed: %d\n", test_passed);
	debug_printf("Failed: %d\n", test_failed);

	if (test_failed == 0) {
		debug_printf("\n✅ All BlockingQueue tests PASSED!\n");
	} else {
		debug_printf("\n❌ Some BlockingQueue tests FAILED!\n");
	}

	debug_printf("========================================\n\n");
}


